#include "initial_alignment.h"

using namespace Eigen;

void InitAlign::set(const Eigen::MatrixXd& accel, const Eigen::MatrixXd& gyro, double gravity, double lat)
{
	_accel = accel;
	_gyro = gyro;
	_gravity = gravity;
	_lat = lat;
}

Eigen::Matrix3d InitAlign::initAlign()
{
	Matrix3d rotmat_cbn;
	Vector3d accel_average = _accel.colwise().mean();
	Vector3d gyro_average = _gyro.colwise().mean();
	Vector3d v_construct = accel_average.cross(gyro_average);

	Vector3d accel_n, gyro_n, v_n;
	accel_n << 0, 0, -(this->_gravity);
	gyro_n << OMEGA * cos(this->_lat), 0, -OMEGA * sin(this->_lat);
	v_n = accel_n.cross(gyro_n);

	Matrix3d n_mat, b_mat;	//constructed matrix for alignment in nav frame and sensor frame
	n_mat << accel_n.transpose(),
		gyro_n.transpose(),
		v_n.transpose();
	b_mat << accel_average.transpose(),
		gyro_average.transpose(),
		v_construct.transpose();

	rotmat_cbn = n_mat.inverse() * b_mat;
	//iteration to normalize the rotation matrix C_b^n
	for (int i = 0; i < 5; i++)
	{
		rotmat_cbn = (rotmat_cbn + rotmat_cbn.transpose().inverse()) * 0.5;
	}		
	return rotmat_cbn;
}

Vector3d RotationMatrixToEuler(const Matrix3d & rot_mat)
{
	Vector3d euler_angle;
	euler_angle(0) = atan2(rot_mat(2, 1), rot_mat(2, 2));
	euler_angle(1) = atan2(-rot_mat(2, 0), sqrt(rot_mat(2, 1) * rot_mat(2, 1) + rot_mat(2, 2) * rot_mat(2, 2)));
	euler_angle(2) = atan2(rot_mat(1, 0), rot_mat(0, 0));
	return euler_angle;
}

Quaterniond EulerToQuaternion(const Vector3d & euler)
{
	double half_phi = euler(0) / 2.0;
	double half_theta = euler(1) / 2.0;
	double half_psi = euler(2) / 2.0;

	//c1 means cos(halfPhi), c2 means cos(halfTheta), c3 means cos(halfPsi)
	//s1 means sin(halfPhi), s2 means sin(halfTheta), s3 means sin(halfPsi)
	double s1 = sin(half_phi);
	double s2 = sin(half_theta);
	double s3 = sin(half_psi);
	double c1 = cos(half_phi);
	double c2 = cos(half_theta);
	double c3 = cos(half_psi);

	Quaterniond q;
	q.w() = c1 * c2 * c3 + s1 * s2 * s3;
	q.x() = s1 * c2 * c3 - c1 * s2 * s3;
	q.y() = c1 * s2 * c3 + s1 * c2 * s3;
	q.z() = c1 * c2 * s3 - s1 * s2 * c3;

	return q;
}
